{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<center><img src=\"../logo.png\" alt=\"Header\" style=\"width: 800px;\"/></center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "@Copyright (C): 2010-2020, Shenzhen Yahboom Tech  \n",
    "@Author: Liusen  \n",
    "@Date: 2020-02-04 16:20:02  \n",
    "@LastEditors: Liusen  \n",
    "@LastEditTime: 2020-02-04 16:20:02   "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# -*- coding:UTF-8 -*-\n",
    "import RPi.GPIO as GPIO\n",
    "import time\n",
    "\n",
    "# 小车电机引脚定义\n",
    "IN1 = 20\n",
    "IN2 = 21\n",
    "IN3 = 19\n",
    "IN4 = 26\n",
    "ENA = 16\n",
    "ENB = 13\n",
    "\n",
    "# 小车按键定义\n",
    "key = 8\n",
    "\n",
    "# 超声波引脚定义\n",
    "EchoPin = 0\n",
    "TrigPin = 1\n",
    "\n",
    "# RGB三色灯引脚定义\n",
    "LED_R = 22\n",
    "LED_G = 27\n",
    "LED_B = 24\n",
    "\n",
    "# 舵机引脚定义\n",
    "ServoPin = 23\n",
    "\n",
    "# 红外避障引脚定义\n",
    "AvoidSensorLeft = 12\n",
    "AvoidSensorRight = 17\n",
    "\n",
    "# 设置GPIO口为BCM编码方式\n",
    "GPIO.setmode(GPIO.BCM)\n",
    "\n",
    "# 忽略警告信息\n",
    "GPIO.setwarnings(False)\n",
    "\n",
    "\n",
    "# 电机引脚初始化为输出模式\n",
    "# 按键引脚初始化为输入模式\n",
    "# 超声波,RGB三色灯,舵机引脚初始化\n",
    "# 红外避障引脚初始化\n",
    "def init():\n",
    "    global pwm_ENA\n",
    "    global pwm_ENB\n",
    "    global pwm_servo\n",
    "    GPIO.setup(ENA, GPIO.OUT, initial=GPIO.HIGH)\n",
    "    GPIO.setup(IN1, GPIO.OUT, initial=GPIO.LOW)\n",
    "    GPIO.setup(IN2, GPIO.OUT, initial=GPIO.LOW)\n",
    "    GPIO.setup(ENB, GPIO.OUT, initial=GPIO.HIGH)\n",
    "    GPIO.setup(IN3, GPIO.OUT, initial=GPIO.LOW)\n",
    "    GPIO.setup(IN4, GPIO.OUT, initial=GPIO.LOW)\n",
    "    GPIO.setup(key, GPIO.IN)\n",
    "    GPIO.setup(EchoPin, GPIO.IN)\n",
    "    GPIO.setup(TrigPin, GPIO.OUT)\n",
    "    GPIO.setup(LED_R, GPIO.OUT)\n",
    "    GPIO.setup(LED_G, GPIO.OUT)\n",
    "    GPIO.setup(LED_B, GPIO.OUT)\n",
    "    GPIO.setup(ServoPin, GPIO.OUT)\n",
    "    GPIO.setup(AvoidSensorLeft, GPIO.IN)\n",
    "    GPIO.setup(AvoidSensorRight, GPIO.IN)\n",
    "    # 设置pwm引脚和频率为2000hz\n",
    "    pwm_ENA = GPIO.PWM(ENA, 2000)\n",
    "    pwm_ENB = GPIO.PWM(ENB, 2000)\n",
    "    pwm_ENA.start(0)\n",
    "    pwm_ENB.start(0)\n",
    "    # 设置舵机的频率和起始占空比\n",
    "    pwm_servo = GPIO.PWM(ServoPin, 50)\n",
    "    pwm_servo.start(0)\n",
    "\n",
    "\n",
    "# 小车前进\t\n",
    "def run(leftspeed, rightspeed):\n",
    "    GPIO.output(IN1, GPIO.HIGH)\n",
    "    GPIO.output(IN2, GPIO.LOW)\n",
    "    GPIO.output(IN3, GPIO.HIGH)\n",
    "    GPIO.output(IN4, GPIO.LOW)\n",
    "    pwm_ENA.ChangeDutyCycle(leftspeed)\n",
    "    pwm_ENB.ChangeDutyCycle(rightspeed)\n",
    "\n",
    "\n",
    "# 小车后退\n",
    "def back(leftspeed, rightspeed):\n",
    "    GPIO.output(IN1, GPIO.LOW)\n",
    "    GPIO.output(IN2, GPIO.HIGH)\n",
    "    GPIO.output(IN3, GPIO.LOW)\n",
    "    GPIO.output(IN4, GPIO.HIGH)\n",
    "    pwm_ENA.ChangeDutyCycle(leftspeed)\n",
    "    pwm_ENB.ChangeDutyCycle(rightspeed)\n",
    "\n",
    "\n",
    "# 小车左转\t\n",
    "def left(leftspeed, rightspeed):\n",
    "    GPIO.output(IN1, GPIO.LOW)\n",
    "    GPIO.output(IN2, GPIO.LOW)\n",
    "    GPIO.output(IN3, GPIO.HIGH)\n",
    "    GPIO.output(IN4, GPIO.LOW)\n",
    "    pwm_ENA.ChangeDutyCycle(leftspeed)\n",
    "    pwm_ENB.ChangeDutyCycle(rightspeed)\n",
    "\n",
    "\n",
    "# 小车右转\n",
    "def right(leftspeed, rightspeed):\n",
    "    GPIO.output(IN1, GPIO.HIGH)\n",
    "    GPIO.output(IN2, GPIO.LOW)\n",
    "    GPIO.output(IN3, GPIO.LOW)\n",
    "    GPIO.output(IN4, GPIO.LOW)\n",
    "    pwm_ENA.ChangeDutyCycle(leftspeed)\n",
    "    pwm_ENB.ChangeDutyCycle(rightspeed)\n",
    "\n",
    "\n",
    "# 小车原地左转\n",
    "def spin_left(leftspeed, rightspeed):\n",
    "    GPIO.output(IN1, GPIO.LOW)\n",
    "    GPIO.output(IN2, GPIO.HIGH)\n",
    "    GPIO.output(IN3, GPIO.HIGH)\n",
    "    GPIO.output(IN4, GPIO.LOW)\n",
    "    pwm_ENA.ChangeDutyCycle(leftspeed)\n",
    "    pwm_ENB.ChangeDutyCycle(rightspeed)\n",
    "\n",
    "\n",
    "# 小车原地右转\n",
    "def spin_right(leftspeed, rightspeed):\n",
    "    GPIO.output(IN1, GPIO.HIGH)\n",
    "    GPIO.output(IN2, GPIO.LOW)\n",
    "    GPIO.output(IN3, GPIO.LOW)\n",
    "    GPIO.output(IN4, GPIO.HIGH)\n",
    "    pwm_ENA.ChangeDutyCycle(leftspeed)\n",
    "    pwm_ENB.ChangeDutyCycle(rightspeed)\n",
    "\n",
    "\n",
    "# 小车停止\t\n",
    "def brake():\n",
    "    GPIO.output(IN1, GPIO.LOW)\n",
    "    GPIO.output(IN2, GPIO.LOW)\n",
    "    GPIO.output(IN3, GPIO.LOW)\n",
    "    GPIO.output(IN4, GPIO.LOW)\n",
    "\n",
    "\n",
    "# 按键检测\n",
    "def key_scan():\n",
    "    while GPIO.input(key):\n",
    "        pass\n",
    "    while not GPIO.input(key):\n",
    "        time.sleep(0.01)\n",
    "        if not GPIO.input(key):\n",
    "            time.sleep(0.01)\n",
    "            while not GPIO.input(key):\n",
    "                pass\n",
    "\n",
    "\n",
    "# 超声波函数\n",
    "'''\n",
    "def Distance_test():\n",
    "    GPIO.output(TrigPin,GPIO.HIGH)\n",
    "    time.sleep(0.000015)\n",
    "    GPIO.output(TrigPin,GPIO.LOW)\n",
    "    while not GPIO.input(EchoPin):\n",
    "        pass\n",
    "    t1 = time.time()\n",
    "    while GPIO.input(EchoPin):\n",
    "        pass\n",
    "    t2 = time.time()\n",
    "    print \"distance is %d \" % (((t2 - t1)* 340 / 2) * 100)\n",
    "    time.sleep(0.01)\n",
    "    return ((t2 - t1)* 340 / 2) * 100\n",
    "'''\n",
    "\n",
    "\n",
    "def Distance():\n",
    "    GPIO.output(TrigPin, GPIO.LOW)\n",
    "    time.sleep(0.000002)\n",
    "    GPIO.output(TrigPin, GPIO.HIGH)\n",
    "    time.sleep(0.000015)\n",
    "    GPIO.output(TrigPin, GPIO.LOW)\n",
    "\n",
    "    t3 = time.time()\n",
    "\n",
    "    while not GPIO.input(EchoPin):\n",
    "        t4 = time.time()\n",
    "        if (t4 - t3) > 0.03:\n",
    "            return -1\n",
    "\n",
    "    t1 = time.time()\n",
    "    while GPIO.input(EchoPin):\n",
    "        t5 = time.time()\n",
    "        if (t5 - t1) > 0.03:\n",
    "            return -1\n",
    "\n",
    "    t2 = time.time()\n",
    "    time.sleep(0.01)\n",
    "    #    print \"distance is %d \" % (((t2 - t1)* 340 / 2) * 100)\n",
    "    return ((t2 - t1) * 340 / 2) * 100\n",
    "\n",
    "\n",
    "def Distance_test():\n",
    "    num = 0\n",
    "    ultrasonic = [ ]\n",
    "    while num < 5:\n",
    "        distance = Distance()\n",
    "        while int(distance) == -1:\n",
    "            distance = Distance()\n",
    "            print(\"Tdistance is %f\" % (distance))\n",
    "        while (int(distance) >= 500 or int(distance) == 0):\n",
    "            distance = Distance()\n",
    "            print(\"Edistance is %f\" % (distance))\n",
    "        ultrasonic.append(distance)\n",
    "        num = num + 1\n",
    "        time.sleep(0.01)\n",
    "    print(ultrasonic)\n",
    "    distance = (ultrasonic[ 1 ] + ultrasonic[ 2 ] + ultrasonic[ 3 ]) / 3\n",
    "    print(\"distance is %f\" % (distance))\n",
    "    return distance\n",
    "\n",
    "\n",
    "# 舵机旋转到指定角度\n",
    "def servo_appointed_detection(pos):\n",
    "    for i in range(18):\n",
    "        pwm_servo.ChangeDutyCycle(2.5 + 10 * pos / 180)\n",
    "\n",
    "\n",
    "# 舵机旋转超声波测距避障，led根据车的状态显示相应的颜色\n",
    "def servo_color_carstate():\n",
    "    # 开红灯\n",
    "    GPIO.output(LED_R, GPIO.HIGH)\n",
    "    GPIO.output(LED_G, GPIO.LOW)\n",
    "    GPIO.output(LED_B, GPIO.LOW)\n",
    "    back(20, 20)\n",
    "    time.sleep(0.08)\n",
    "    brake()\n",
    "\n",
    "    # 舵机旋转到0度，即右侧，测距\n",
    "    servo_appointed_detection(0)\n",
    "    time.sleep(0.8)\n",
    "    rightdistance = Distance_test()\n",
    "\n",
    "    # 舵机旋转到180度，即左侧，测距\n",
    "    servo_appointed_detection(180)\n",
    "    time.sleep(0.8)\n",
    "    leftdistance = Distance_test()\n",
    "\n",
    "    # 舵机旋转到90度，即前方，测距\n",
    "    servo_appointed_detection(90)\n",
    "    time.sleep(0.8)\n",
    "    frontdistance = Distance_test()\n",
    "\n",
    "    if leftdistance < 30 and rightdistance < 30 and frontdistance < 30:\n",
    "        # 亮品红色，掉头\n",
    "        GPIO.output(LED_R, GPIO.HIGH)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.HIGH)\n",
    "        spin_right(35, 35)\n",
    "        time.sleep(0.58)\n",
    "    elif leftdistance >= rightdistance:\n",
    "        # 亮蓝色\n",
    "        GPIO.output(LED_R, GPIO.LOW)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.HIGH)\n",
    "        spin_left(35, 35)\n",
    "        time.sleep(0.28)\n",
    "    elif leftdistance <= rightdistance:\n",
    "        # 亮品红色，向右转\n",
    "        GPIO.output(LED_R, GPIO.HIGH)\n",
    "        GPIO.output(LED_G, GPIO.LOW)\n",
    "        GPIO.output(LED_B, GPIO.HIGH)\n",
    "        spin_right(35, 35)\n",
    "        time.sleep(0.28)\n",
    "\n",
    "\n",
    "# 延时2s\t\n",
    "time.sleep(2)\n",
    "\n",
    "# try/except语句用来检测try语句块中的错误，\n",
    "# 从而让except语句捕获异常信息并处理。\n",
    "try:\n",
    "    init()\n",
    "    key_scan()\n",
    "    while True:\n",
    "        distance = Distance_test()\n",
    "        if distance > 50:\n",
    "            # 遇到障碍物,红外避障模块的指示灯亮,端口电平为LOW\n",
    "            # 未遇到障碍物,红外避障模块的指示灯灭,端口电平为HIGH\n",
    "            LeftSensorValue = GPIO.input(AvoidSensorLeft)\n",
    "            RightSensorValue = GPIO.input(AvoidSensorRight)\n",
    "\n",
    "            if LeftSensorValue == True and RightSensorValue == True:\n",
    "                run(35, 35)  # 当两侧均未检测到障碍物时调用前进函数\n",
    "            elif LeftSensorValue == True and RightSensorValue == False:\n",
    "                spin_left(30, 30)  # 右边探测到有障碍物，有信号返回，原地向左转\n",
    "                time.sleep(0.002)\n",
    "            elif RightSensorValue == True and LeftSensorValue == False:\n",
    "                spin_right(30, 30)  # 左边探测到有障碍物，有信号返回，原地向右转\n",
    "                time.sleep(0.002)\n",
    "            elif RightSensorValue == False and LeftSensorValue == False:\n",
    "                spin_right(30, 30)  # 当两侧均检测到障碍物时调用固定方向的避障(原地右转)\n",
    "                time.sleep(0.002)\n",
    "                run(35, 35)\n",
    "                GPIO.output(LED_R, GPIO.LOW)\n",
    "                GPIO.output(LED_G, GPIO.HIGH)\n",
    "                GPIO.output(LED_B, GPIO.LOW)\n",
    "        elif 30 <= distance <= 50:\n",
    "            # 遇到障碍物,红外避障模块的指示灯亮,端口电平为LOW\n",
    "            # 未遇到障碍物,红外避障模块的指示灯灭,端口电平为HIGH\n",
    "            LeftSensorValue = GPIO.input(AvoidSensorLeft)\n",
    "            RightSensorValue = GPIO.input(AvoidSensorRight)\n",
    "\n",
    "            if LeftSensorValue == True and RightSensorValue == True:\n",
    "                run(35, 35)  # 当两侧均未检测到障碍物时调用前进函数\n",
    "            elif LeftSensorValue == True and RightSensorValue == False:\n",
    "                spin_left(30, 30)  # 右边探测到有障碍物，有信号返回，原地向左转\n",
    "                time.sleep(0.002)\n",
    "            elif RightSensorValue == True and LeftSensorValue == False:\n",
    "                spin_right(30, 30)  # 左边探测到有障碍物，有信号返回，原地向右转\n",
    "                time.sleep(0.002)\n",
    "            elif RightSensorValue == False and LeftSensorValue == False:\n",
    "                spin_right(30, 30)  # 当两侧均检测到障碍物时调用固定方向的避障(原地右转)\n",
    "                time.sleep(0.002)\n",
    "                run(35, 35)\n",
    "        elif distance < 30:\n",
    "            servo_color_carstate()\n",
    "\n",
    "except KeyboardInterrupt:\n",
    "    pass\n",
    "pwm_ENA.stop()\n",
    "pwm_ENB.stop()\n",
    "GPIO.cleanup()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
